
import Jama.Matrix;

import com.mathworks.toolbox.javabuilder.MWClassID;
import com.mathworks.toolbox.javabuilder.MWException;
import com.mathworks.toolbox.javabuilder.MWNumericArray;

import clubhouse.ekf.*;

import trajectory.trajectoryclass;

public class ekf  {
	
	private int sensors = 5;
	private double[] xS = new double[] {0, 20, 35, -15, 10};
	private double[] yS = new double[] {30, 25, -5, 5, 10};
	
	private double dt = 0.01;
	private double sd = 0.05;
	
	private double[][] m = new double[][] {{0},{2},{1},{0}};
	private double[][] p = new double[][] {
			{4, 0, 0, 0},
			{0, 4, 0, 0},
			{0, 0, 4, 0},
			{0, 0, 0, 4}};
	/*
	private double[][] r = new double[][] {
			{sd*sd, 0, 0, 0, 0},
			{0, sd*sd, 0, 0, 0},
			{0, 0, sd*sd, 0, 0},
			{0, 0, 0, sd*sd, 0},
			{0, 0, 0, 0, sd*sd}};
	private double[][] v = new double[][] {
			{1, 0, 0, 0, 0},
			{0, 1, 0, 0, 0},
			{0, 0, 1, 0, 0},
			{0, 0, 0, 1, 0},
			{0, 0, 0, 0, 1}};
	*/
	
	private trajectoryclass toto;
	private functions c;
	private filter f;
	private util u;
	
	private MWNumericArray A;
	private MWNumericArray Q;
	
	private MWNumericArray M = new MWNumericArray(m, MWClassID.DOUBLE);
	private MWNumericArray P = new MWNumericArray(p, MWClassID.DOUBLE);
	private MWNumericArray R /*= new MWNumericArray(r, MWClassID.DOUBLE)*/;
	private MWNumericArray V /*= new MWNumericArray(v, MWClassID.DOUBLE)*/;
	
	private MWNumericArray X;
	private MWNumericArray Y;
	//private MWNumericArray S = new MWNumericArray(new double[][] {s1, s2, s3, s4, s5}, MWClassID.DOUBLE);
	private MWNumericArray S = new MWNumericArray(new double[][] {xS, yS}, MWClassID.DOUBLE);
	
	//private double[][] mm;
	//private double[][] mp;
	//private double[][] me;
	
	private MWNumericArray tmp_X;
	//private MWNumericArray tmp_dY;
	//private MWNumericArray tmp_Y;
	
	
	public ekf() {
		try {
			c = new functions();
			f = new filter();
			u = new util();
		} catch (MWException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		this.X = new MWNumericArray(new double[][] {{0}, {0}, {0}, {0}}, MWClassID.DOUBLE);
	}
	
	public void trajectory() {
		double[][] a = new double[1][900];
		int i;
		for(i=0; i<900; i++) {
			a[0][i] = 0;
		}
		a[0][300] = Math.PI/2/dt;
		a[0][500] = Math.PI/2/dt;
		a[0][700] = Math.PI/2/dt;

		MWNumericArray m = new MWNumericArray(a, MWClassID.DOUBLE);
		MWNumericArray x = new MWNumericArray(new double[][] {{0},{2},{1},{0}}, MWClassID.DOUBLE);
		System.out.print("calculating trajectory...\n");
		Object[] rhs = new Object[] {tmp_X};
		Object[] lhs = new Object[] {m,x, dt};
		try {
			toto = new trajectoryclass();
			toto.calculate_trajectory(rhs,lhs);
		} catch (MWException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		System.out.print("calculating trajectory complete\n");
		tmp_X = (MWNumericArray) rhs[0];
		//System.out.print(tmp_X.toString());
		//int[] dim = tmp_X.getDimensions();
		//double[][] xk = (double[][]) tmp_X.toDoubleArray();
		//for (i=0; i<dim[1]; i++){
		//	System.out.print("Instant "+i+": x="+xk[0][i]+" y="+xk[1][i]+"\n");
		//}
		m.dispose();
		x.dispose();
	}
	
	public void initCalcul() {
		double[][] f = new double[][]{
				{0, 0, 1, 0},
				{0, 0, 0, 1},
				{0, 0, 0, 0},
				{0, 0, 0, 0}};
		double[][] l = new double[][]{
				{1, 0, 0, 0},
				{0, 1, 0, 0},
				{0, 0, 1, 0},
				{0, 0, 0, 1}};
		double[][] qc = new double[][]{
				{0, 0, 0, 0},
				{0, 0, 0, 0},
				{0, 0, 0.1, 0},
				{0, 0, 0, 0.1}};
		
		MWNumericArray F = new MWNumericArray(f, MWClassID.DOUBLE);
		MWNumericArray L = new MWNumericArray(l, MWClassID.DOUBLE);
		MWNumericArray Qc = new MWNumericArray(qc, MWClassID.DOUBLE);
		
		Object[] rhs = new Object[] {this.A, this.Q};
		Object[] lhs = new Object[] {F, L, Qc, this.dt};
		
		try {
			System.out.println("calculating lti_disc...");
			u.lti_disc(rhs, lhs);
			System.out.println("calculating lti_disc complete");
			this.A = (MWNumericArray) rhs[0];
			this.Q = (MWNumericArray) rhs[1];
		} catch (MWException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		F.dispose();
		L.dispose();
		Qc.dispose();
	}
	
	public void measurement() {
		Object[] rhs = new Object[] {this.Y};
		Object[] lhs = new Object[] {this.X, this.S};
		
		try {
			c.bot_h(rhs, lhs);
			this.Y = (MWNumericArray) rhs[0];
		} catch (MWException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
	}
	
	public double[][] getMEstimate() {
		return (double[][]) this.M.toDoubleArray();
	}
	
	public double[][] getPEstimate() {
		return (double[][]) this.P.toDoubleArray();
	}
	
	public void calcul() {
		Object[] rhs = new Object[] {this.M, this.P};
		Object[] lhs = new Object[] {this.M, this.P, this.A, this.Q};
		try {
			f.ekf_predict1(rhs, lhs);
			this.M = (MWNumericArray) rhs[0];
			this.P = (MWNumericArray) rhs[1];
		} catch (MWException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		/*
		//temp dY
		Object[] temp_rhs = new Object[] {this.tmp_dY};
		Object[] temp_lhs = new Object[] {this.M, this.S};
		try {
			c.bot_dh_dx(temp_rhs, temp_lhs);
			this.tmp_dY = (MWNumericArray) rhs[0];
		} catch (MWException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
		
		//temp Y
		Object[] temp_rhs2 = new Object[] {this.tmp_Y};
		Object[] temp_lhs2 = new Object[] {this.M, this.S};
		try {
			c.bot_h(temp_rhs2, temp_lhs2);
			this.tmp_Y = (MWNumericArray) rhs[0];
		} catch (MWException e1) {
			// TODO Auto-generated catch block
			e1.printStackTrace();
		}
		*/
		
		setRandV(sensors);
		
		String dh = "bot_dh_dx";
		String h = "bot_h";
		
		Object[] lhs2 = new Object[] {this.M, this.P, this.Y, dh/*this.tmp_dY*/, this.R, h/*this.tmp_Y*/, this.V, this.S};
		try {
			f.ekf_update1(rhs, lhs2);
			this.M = (MWNumericArray) rhs[0];
			this.P = (MWNumericArray) rhs[1];
		} catch (MWException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}
	
	public void setMotion(double x, double y, double dx, double dy) {
		int[] index = new int[] {0, 1};
		index[0] = 1;
		this.X.set(index, x);
		index[0] = 2;
		this.X.set(index, y);
		index[0] = 3;
		this.X.set(index, dx);
		index[0] = 4;
		this.X.set(index, dy);
	}
	
	public void setRandV(int size) {
		this.R.dispose();
		this.V.dispose();
		
		this.R = new MWNumericArray((Matrix.identity(size, size).timesEquals(sd*sd)).getArray(), MWClassID.DOUBLE);
		this.V = new MWNumericArray((Matrix.identity(size, size)).getArray(), MWClassID.DOUBLE);
	}
	
	public void debug() {
		int i;
		int[] index = new int[] {0, 1};
		int[] dim = tmp_X.getDimensions();
		double[][] xk = (double[][]) tmp_X.toDoubleArray();
		X = new MWNumericArray(new double[][] {{0}, {0}, {0}, {0}}, MWClassID.DOUBLE);
		for (i=0; i<dim[1]; i++){
			System.out.println("Setting new coordinates");
			//X = new MWNumericArray(new double[][] {{xk[0][i]}, {xk[1][i]}, {xk[2][i]}, {xk[3][i]}}, MWClassID.DOUBLE);
			index[0] = 1;
			X.set(index, xk[0][i]);
			index[0] = 2;
			X.set(index, xk[1][i]);
			index[0] = 3;
			X.set(index, xk[2][i]);
			index[0] = 4;
			X.set(index, xk[3][i]);
			System.out.println("Calculating measurements");
			measurement();
			System.out.println("Predicting and updating");
			calcul();
			double[][] tmp  = getMEstimate();
			System.out.println("x: "+tmp[0][0]+" y: "+tmp[1][0]+" dx: "+tmp[2][0]+" dy: "+tmp[3][0]);
			System.out.println("== end cycle ==");
		}
	}
	
	public static void main(String[] args) {
		
		ekf test = new ekf();
		test.initCalcul();
		test.trajectory();
		test.debug();
		/*
		test.debug();
		test.initCalcul();
		test.trajectory();
		 try {
			ekfclass testbis = new ekfclass();
			testbis.ekfs_bot_demo_original();
		} catch (MWException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		*/
	}
}
